//
//  MapPoint.cpp
//  markerARLib
//
//  Created by fg on 5/11/16.
//  Copyright © 2016 CvidKal. All rights reserved.
//

#include "MapPoint.hpp"




namespace markerAR
{
    

Eigen::Matrix<double, 2, 3> MapPoint::dx_dX()
{
    Eigen::Matrix<double, 2, 3> ret;
    double a,b;
    a = 1./pointXYZ.z();
    b = a*a;
    ret(0,0) = a;
    ret(1,1) = a;
    ret(0,2) = -1.*pointXYZ.x()*b;
    ret(1,2) = -1.*pointXYZ.y()*b;
    return ret;
    
}




Eigen::Matrix<double,3,6>* MapPoint::getPatialJ()
{
    if(patialj == nullptr)
    {
        patialj = new Eigen::Matrix<double, 3, 6>();
        patialj->setZero();
        patialj->operator()(0,1) = pointXYZ.z();
        patialj->operator()(1,0) = -pointXYZ.z();
        patialj->operator()(0,2) = -pointXYZ.y();
        patialj->operator()(2,0) = pointXYZ.y();
         patialj->operator()(1.2) = pointXYZ.x();
         patialj->operator()(2,1) = -pointXYZ.x();
         patialj->operator()(0,0) = 1;
         patialj->operator()(1,1) = 1;
         patialj->operator()(2,2) = 1;
        return patialj;
    }
    return patialj;
}
}